#coding=utf-8
import socket
import time
import threading
import communicate
import pygame
# 创建连接
flag = "not received"
time.sleep(5)
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# 绑定端口
s.bind(('192.168.137.10', 24))
print('Waiting for connection...')
i = 0

FB = 0
LR = 0
UD = 0
SH = 0
SpeedFB = 0
SpeedLR = 0
SpeedUD = 0
SpeedSH = 0
check = 0
POS = 0
PIT = 0
SpeedPIT = 0
ROL = 0
SpeedROL = 0
def sendcmd():
    global led
    
    #print("UD"+str(UD)+"SpeedUD"+str(SpeedUD))
    #print("FB"+str(FB)+"SpeedFB"+str(SpeedFB))
    #print("LR"+str(LR)+"SpeedLR"+str(SpeedLR))
    led = 0
    if flag == "received":
        communicate.sendMsg(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,POS,PIT,SpeedPIT,ROL,SpeedROL,0,0)
        print(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,POS,PIT,SpeedPIT,ROL,SpeedROL,0,0,check)
        if(led == 4):
            led = 0
    t = threading.Timer(0.005,sendcmd)
    t.start()
    
t = threading.Timer(0.005,sendcmd)
t.start()

while True:
    try:
        # 无数据接收则线程挂起，等待数据
        data, addr = s.recvfrom(1024)
        # print('Received from %s:%s.' % addr)
        cmd = data.decode("UTF-8")
        # print(cmd)
        if cmd[0:2] == "ST" and cmd[16:18] == "ED":
            UD = ord(cmd[2])
            SpeedUD = ord(cmd[3])
            FB = ord(cmd[4])
            SpeedFB = ord(cmd[5])
            LR = ord(cmd[6])
            SpeedLR = ord(cmd[7])
            SH = ord(cmd[8])
            SpeedSH = ord(cmd[9])
            POS = ord(cmd[10])
            PIT = ord(cmd[11])
            SpeedPIT = ord(cmd[12])
            ROL = ord(cmd[13])
            SpeedROL = ord(cmd[14])
            check = ord(cmd[15])
            local_chk = communicate.verify(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,\
                                           POS,PIT,SpeedPIT,ROL,SpeedROL)
           # print(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,POS,PIT,SpeedPIT,ROL,SpeedROL,check)
            if(check == local_chk):
                flag = "received"
                local_chk = 0
            else :
                flag = "wrong"
        else:
            flag = "format error"
    except:
        flag = "receive error"
    # print(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,POS,PIT,SpeedPIT,ROL,SpeedROL,check)
    # print(flag)
    # event=pygame.event.get()
    # if event.type == pygame.KEYDOWN  and event.key == pygame.K_ESC:
    #     s.close()

        